#include "turcle_pose_info/srv/turtle_pose_info.hpp"
#include <chrono>
#include <rclcpp/rclcpp.hpp>
#include <unistd.h>

using namespace std::chrono_literals;

class TurtlePoseClient : public rclcpp::Node
{
private:
    rclcpp::Client<turcle_pose_info::srv::TurtlePoseInfo>::SharedPtr
        _pose_client;

    rclcpp::TimerBase::SharedPtr _timer;

    void _control_callback()
    {
        RCLCPP_INFO(this->get_logger(), "timer invoke");
        while (!_pose_client->wait_for_service(1s))
        { /* code */
            if (!rclcpp::ok())
            {
                RCLCPP_ERROR(this->get_logger(), "interrupt");
            }
            sleep(1);
            RCLCPP_INFO(this->get_logger(), "wait server");
        }
        // RCLCPP_INFO(this->get_logger(), "debug 1");
        auto request =
            std::make_shared<turcle_pose_info::srv::TurtlePoseInfo::Request>();
        request->target_x = rand() % 15;
        request->target_y = rand() % 15;
        // RCLCPP_INFO(this->get_logger(), "debug 2");

        RCLCPP_INFO(this->get_logger(), "generate pose %f/%f",
                    request->target_x, request->target_y);
        _pose_client->async_send_request(
            request, std::bind(&TurtlePoseClient::_response_call, this,
                               std::placeholders::_1));
    }
    void _response_call(
        rclcpp::Client<turcle_pose_info::srv::TurtlePoseInfo>::SharedFuture
            result)
    {
        auto response = result.get();
        if (response->result ==
            turcle_pose_info::srv::TurtlePoseInfo::Response::FAIL)
        {
            RCLCPP_INFO(this->get_logger(), "set pose failed");
        }
        else
        {
            RCLCPP_INFO(this->get_logger(), "set pose success");
        }
    }

public:
    TurtlePoseClient(const std::string &node_name) : rclcpp::Node(node_name)
    {
        _pose_client =
            this->create_client<turcle_pose_info::srv::TurtlePoseInfo>(
                "/pose_service");
        _timer = this->create_wall_timer(
            20s, std::bind(&TurtlePoseClient::_control_callback, this));
    }
    ~TurtlePoseClient() {}
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<TurtlePoseClient>("turtle_pose_client");
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}